#!/bin/bash

# =============================
# 颜色定义（用于脚本中的高亮）
# =============================
RED='\033[0;31m'
GREEN='\033[0;32m'
YELLOW='\033[1;33m'
NC='\033[0m' # No Color

# =============================
# 开始扫描自定义话题（robot_msgs/*）
# =============================
echo -e "${YELLOW}正在扫描自定义话题 (只筛选消息类型为 robot_msgs/*)...${NC}"

# 先把所有话题逐行读入数组
mapfile -t all_topics < <(rostopic list)

custom_topics=()
for topic in "${all_topics[@]}"; do
    # 直接调用 rostopic type 来获取该话题的消息类型
    # 如果该话题没有任何发布者/订阅者，rostopic type 也会返回错误，我们这里用 2>/dev/null 屏蔽
    msg_type=$(rostopic type "$topic" 2>/dev/null)
    if [[ "$msg_type" == robot_msgs/* ]]; then
        custom_topics+=("$topic")
        echo -e "发现自定义话题: ${GREEN}$topic${NC} (类型: $msg_type)"
    fi
done

# 如果找不到任何符合条件的自定义话题，则提示并退出
if [ ${#custom_topics[@]} -eq 0 ]; then
    echo -e "${RED}未找到任何自定义话题！请确认：${NC}"
    echo -e "  1. 你的 ROS 节点已正常发布或订阅这些话题。"
    echo -e "  2. 消息类型确实是 robot_msgs/xxx。"
    exit 1
fi

echo -e "${YELLOW}共找到 ${#custom_topics[@]} 个自定义话题，准备开始监听...${NC}"
echo -e "${GREEN}提示：按 Ctrl+C 停止监听并退出脚本${NC}"

# =============================
# 捕获 SIGINT（Ctrl+C），确保能杀掉所有后台 rostopic echo 进程
# =============================
pids=()
cleanup() {
    echo -e "\n${YELLOW}正在停止所有监听进程...${NC}"
    for pid in "${pids[@]}"; do
        kill "$pid" 2>/dev/null
    done
    echo -e "${YELLOW}已退出监听，脚本结束。${NC}"
    exit 0
}
trap cleanup SIGINT SIGTERM

# =============================
# 为每个自定义话题启动后台监听
# =============================
for topic in "${custom_topics[@]}"; do
    echo -e "\n===== ${GREEN}Topic: $topic${NC} ====="
    # 直接调用 rostopic echo 并放到后台，输出会混合到当前终端
    rostopic echo "$topic" &
    pids+=("$!")
done

# =============================
# 脚本在此挂起，直到收到 Ctrl+C 信号
# =============================
# 因为我们用 trap 捕获了 SIGINT，用户按 Ctrl+C 时会进入 cleanup() 并退出
while true; do
    sleep 1
done

